#include"my_test/test2_astar.h"

Astar::Astar(Node* startPos,Node* endPos, int* map,int sizex,int sizey,int sizez)
{
    //起始点，终点
    this->startPos=startPos;
    this->endPos=endPos;

    //大小
    this->sizex=sizex;
    this->sizey=sizey;
    this->sizez=sizez;
    //初始化pMap的行和列，因为其行和列的数目是不定的
    this->pMap=(int **)malloc(sizex*sizeof(int *));
    for(int i=0;i<sizex;i++)
    {
        this->pMap[i]=(int* )malloc(sizey*sizeof(int));
    }
    //初始化pMap，如果z上有，则为1，此时在
    for (int x=0;x<sizex;x++)
    {
        for(int y=0;y<sizey;y++)
        {
            pMap[x][y]=0;
            for(int z=10;z<=12;z++)
            {
                int index=z*sizex*sizey+y*sizex+x; //是一个从三维地图到一位数列的索引
                if(map[index]>70) //建立地图的时候，当前index如果有70次，则认为其是障碍物
                {
                    pMap[x][y]=1;
                }
            }
        }
    }

    //初始点和终点是没有障碍物的
    pMap[startPos->x][startPos->y]=0;
    pMap[endPos->x][endPos->y]=0;
}

Astar::~Astar()
{}

//检验当前点是否是可行的
bool Astar::isInvalid(int x,int y)
{
    if(x<0||x>sizex||y<0||y>sizey)
    {
        return false;
    }
    return true;
}

//检验当前位置是否是障碍物obstacle
bool Astar::isObs(int x,int y)
{
    int thr=3;
    if(x<thr||x+thr>sizex||y<thr||y+thr>sizey)return true;//边界都认为是障碍物
    //以3为范围，对是否是障碍物进行判断，如果3个内有障碍物，则认为该点不可行
    for(int dx=-thr;dx<=thr;dx++)
    {
        for(int dy=-thr;dy<=thr;dy++)
        {
            int idx=x+dx;
            int idy=y+dy;
            if(pMap[idx][idy]==1)
            {
                return true;
            }
        }
    }
    return false;
}

//看输入的点xy是否在nodelist中，是的话返回其在哪一位，不是的话返回-1
int Astar::isContains(vector<Node*>*Nodelist,int x,int y)
{
    for(int i=0;i<(int)Nodelist->size();i++)
    {
        if(Nodelist->at(i)->x==x&&Nodelist->at(i)->y==y)
        {
            return i;
        }
    }
    return -1;
}

//计算输入节点的GHF
//F是总距离
//G是从起点到目标点到的距离，通过各个father节点走过来
//H是目标点到终点的距离
void Astar::countGHF(Node* sNode,Node* eNode,int g)
{
    int h1=abs(sNode->x-eNode->x);
    int h2=abs(sNode->y-eNode->y);
    int h=h1+h2;
    int currentg=sNode->father->g+g;
    int f=currentg+h;
    sNode->f=f;
    sNode->h=h;
    sNode->g=currentg;
}

//比较两个节点，那个的F值更小
bool Astar::compare(Node* n1,Node* n2)
{
    return n1->f<n2->f;
}

//对输入的点进行检验和核实，包括更新父节点等操作，然后push进openlist
void Astar::checkPoint(int x,int y,Node* father,int g)
{
    if(!isInvalid(x,y))return;          //检验该点是否不合理
    if(isObs(x,y))return;          //检验该点是否是障碍
    if(isContains(&closeList,x,y)!=-1)return;           //检验该点是否已经open，已经open的话就return


    int index;
    //如果这个节点在openlist中，则对其进行核实，更新gather节点
    if((index=isContains(&openList,x,y))!=-1)
    {
        Node* point=openList[index];
        //如果该点已经open了，则对其进行核实
        //如果原来的g，不如从father，在此输入是currentPoint，走过去更近，则将其father更新为输入的点
        if(point->g>father->g+g)
        {
            point->father=father;
            point->g=father->g+g;
            point->f=point->g+point->h;
        }
    }

    //如果没有open，则创建，其father节点为currentPoint
    else
    {
        Node* point=new Node(x,y,father);
        countGHF(point,endPos,g);
        openList.push_back(point);
    }
}


//扩展当前点周围的8个点
void Astar::NextStep(Node* currentPoint)
{
    checkPoint(currentPoint->x-1,currentPoint->y,currentPoint,1);
    checkPoint(currentPoint->x+1,currentPoint->y,currentPoint,1);
    checkPoint(currentPoint->x,currentPoint->y-1,currentPoint,1);
    checkPoint(currentPoint->x,currentPoint->y+1,currentPoint,1);

   checkPoint(currentPoint->x+1,currentPoint->y+1,currentPoint,1.414);
   checkPoint(currentPoint->x+1,currentPoint->y-1,currentPoint,1.414);
    checkPoint(currentPoint->x-1,currentPoint->y+1,currentPoint,1.414);
    checkPoint(currentPoint->x-1,currentPoint->y-1,currentPoint,1.414);
}


//search是主程序
vector<Node*>Astar::search()
{
    cout<<"search"<<endl;
    //如果起点或者终点是障碍物，直接return，无法计算
    if(isObs(startPos->x,startPos->y)||isObs(endPos->x,endPos->y))return path;
    Node* current;
    openList.push_back(startPos);//添加在list的尾部

    while(openList.size()>0)
    {
        current=openList[0];
        if(current->x==endPos->x&&current->y==endPos->y)
        {
            cout<<"find the path"<<endl;
            printPath(current);
            printMap();
            break;
        }
        NextStep(current);//扩展到周围的点，并且push进openlist
        closeList.push_back(current);//将当前点放进closelist
        openList.erase(openList.begin());//将当前点从openlist中删除
        sort(openList.begin(),openList.end(),compare);//对list里的点进行排序，F最小的在前面
    }
    return path;
}

//打印地图
void Astar::printMap()
{
    for(int i=0;i<sizex;i++)
    {
        for(int j=0;j<sizey;j++)
        {
            if(pMap[i][j]==0)
            {cout<<pMap[i][j]<<" ";}
            else if(pMap[i][j]==1)
            {cout<<"*"<<" ";}
            else
            {cout<<"+"<<" ";}
            
        }
        cout<<endl;
    }
}

//打印路径
void Astar::printPath(Node* current)
{
    if(current->father!=nullptr)
    {
        printPath(current->father);
    }
    cout<<"x:"<<current->x<<"y:"<<current->y<<endl;
    pMap[current->x][current->y]=6;

    Node* point= new Node(current->x,current->y);\
    path.push_back(point);
}

